#include "DTM437.h"

DTM437::DTM437(){
    memset(&frame, '\0', sizeof(frame));
    frame.Mxe.Priority = 3;
    frame.Mxe.PDUSpecific = 0xEB;
}

bool DTM437::init()
{
    if(can_driver_init())
    {
        CanDriver::get()->set_terminal_resistor();
        J1939_Initialization();
        poll_thread = boost::make_shared<boost::thread>(boost::bind(&DTM437::ms10_loop, this));
        return true;
    }
    else
    {
        return false;
    }
}

bool DTM437::can_driver_init()
{
    return CanDriver::get()->init();
}

void DTM437::ms10_loop()
{
    std::cout << "Loop(10ms) Start." << std::endl;
    while ( CanDriver::get()->ready)
    {
        try
        {
            boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
            J1939_Poll();
        }
        catch (std::exception &e)
        {
            std::cout << "Abort" << e.what() << std::endl;
        }
    }
    std::cout << "Loop(10ms) Terminate." << std::endl;
}

void DTM437::async_access_param(uint16_t param, uint8_t wr_rd, const uint8_t* data)
{
    static DTM437_Frame_T _dtm_frame;
    int i = 0;
    _dtm_frame.s_.param = param;
    _dtm_frame.s_.wr_rd = wr_rd;
    _dtm_frame.s_.status = 0x00;
    for(i=0;i<4;i++)
        _dtm_frame.s_.data[i] = data[i];
    for(i=0;i<8;i++)
        this->frame.Mxe.Data[i] = _dtm_frame.buf[i];
    frame.Mxe.PDUFormat = 0xEF;
    J1939_Send_Message(&frame, 0);
}

void DTM437::async_get_status()
{
    uint8_t zeros[4] = {0};
    this->async_access_param(DTM437_DeviceStatus, DTM437_RD, zeros);
}

void DTM437::stop()
{
    CanDriver::get()->de_init();
}

void DTM437::async_get_rfid_data(uint16_t tag_addr, uint16_t len)
{
    uint8_t data[4];
    memcpy(data, &tag_addr, 2);
    memcpy(data+2, &len, 2);
    this->async_access_param(DTM437_TagData, DTM437_RD, data);
}

void DTM437::async_set_rfid_data(uint16_t tag_addr, uint8_t* data, uint16_t len)
{
    DTM437_Frame_T _dtm_frame;
    uint8_t* buf = new uint8_t[8+len];
    int i = 0;
    _dtm_frame.s_.param = DTM437_TagData;
    _dtm_frame.s_.wr_rd = DTM437_WR;
    _dtm_frame.s_.status = 0x00;
    memcpy(_dtm_frame.s_.data, &tag_addr, 2);
    memcpy(_dtm_frame.s_.data+2, &len, 2);
    for(i=0;i<8;i++)
        buf[i] = _dtm_frame.buf[i];
    for(i=0;i<len;i++)
        buf[i+8] = data[i];
    J1939_TP_TX_Message(0xEF00, 0xEB, buf, len+8 ,0);
    delete[] buf;
}